// QuaternionsOperation.cpp


#include "QuaternionsLib.h"
#include "VectorsLib.h"


using namespace std;

namespace Quaternions
{
	//quaternion: x,y,z,w
	float vector[4];


	////////////////////////////////
	/* Read Quaternion
	*/
	////////////////////////////////
	void MyQuaternions::ReadQuaternionAngle(float *axis, float degree)
	{
		degree = degree * 0.5f;
		float angle = sin(degree*3.14159 / 180);
		float vectorTemp[3];
		memcpy(vectorTemp, Vectors::MyVectors::NormalizeVector(axis), sizeof(vectorTemp));
		vector[0] = vectorTemp[0] * angle;
		vector[1] = vectorTemp[1] * angle;
		vector[2] = vectorTemp[2] * angle;
		vector[3] = cos(degree*3.14159 / 180);	
	}

	void MyQuaternions::ReadQuaternion(float x, float y, float z, float pw)
	{
		vector[0] = x;
		vector[1] = y;
		vector[2] = z;
		vector[3] = pw;
	}


	////////////////////////////////
	/* Quaternion multiplication
	*/
	////////////////////////////////
	float (*MyQuaternions::Mutiply(float *quaternion2))
	{
		float rv[4];
		rv[0] = vector[3] * quaternion2[0] + vector[0] * quaternion2[3] + vector[1] * quaternion2[2] - vector[2] * quaternion2[1];
		rv[1] = vector[3] * quaternion2[1] + vector[1] * quaternion2[3] + vector[2] * quaternion2[0] - vector[0] * quaternion2[2];
		rv[2] = vector[3] * quaternion2[2] + vector[2] * quaternion2[3] + vector[0] * quaternion2[1] - vector[1] * quaternion2[0];
		rv[4] = vector[3] * quaternion2[3] - vector[0] * quaternion2[0] - vector[1] * quaternion2[1] - vector[2] * quaternion2[2];

		return rv;
	}


	////////////////////////////////
	/* Get Magnitude
	*/
	////////////////////////////////
	float MyQuaternions::GetMagnitude()
	{
		float magnitude = vector[0] * vector[0] + vector[1] * vector[1] + vector[2] * vector[2] + vector[3] * vector[3];
		magnitude = sqrt(magnitude);
		
		return magnitude;
	}


	////////////////////////////////
	/* Normalize quaternion
	*/
	////////////////////////////////
	float (*MyQuaternions::Normalize())
	{
		float result[4];
		float magnitude = GetMagnitude();
		result[0] = vector[0] / magnitude;
		result[1] = vector[1] / magnitude;
		result[2] = vector[2] / magnitude;
		result[3] = vector[3] / magnitude;

		return result;
	}


	////////////////////////////////
	/* Quaternion conjugate
	*/
	////////////////////////////////
	float (*MyQuaternions::GetConjugate())
	{
		float result[4];
		result[0] = -vector[0];
		result[1] = -vector[1];
		result[2] = -vector[2];
		result[3] = vector[3];

		return result;
	}


}